#include "BiCubicInterpolation.h"
#include <cassert>

////////////////////////////////////////////////////////////////////////////////
/////////////////////////PRIVATE METHODS////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
void BiCubicInterpolation::Compute( )
{
   //fit 1D cubic splines with x constant
   
}
////////////////////////////////////////////////////////////////////////////////
////////////////////////PUBLIC METHODS//////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

BiCubicInterpolation::BiCubicInterpolation( )
{
   
}
////////////////////////////////////////////////////////////////////////////////
BiCubicInterpolation::~BiCubicInterpolation( )
{
   //delete spline;
}
////////////////////////////////////////////////////////////////////////////////
void BiCubicInterpolation::SetInputPoints( vector<double> x, vector<double> y, 
                              gsl_matrix* z )
{
   this->x = x;
   this->y = y;
   this->z = z;
   if ( DBG_BICUBIC )
   {
      cout<<"================================================"<<endl;
      cout<<"Size of x :"<<x.size()<<endl;
      cout<<"Size of y :"<<y.size()<<endl;
      cout<<"Size of z :"<<z->size1<<" "<<z->size2<<endl;
      cout<<"================================================"<<endl;
   }
   //check data
   if ( x.size() != z->size1 )
   {
      cout<<"Size of x data and corresponding z data do not match !!!"<<endl;
      exit( -1 );
   }
   if ( y.size() != z->size2 )
   {
      cout<<"Size of y data and corresponding z data do not match !!!"<<endl;
      exit( -1 );
   }
   //++++++++end check data
}
////////////////////////////////////////////////////////////////////////////////
double BiCubicInterpolation::EvaluateAt( double xat, double yat )
{
   //interpolate the bi-cubic spline function at the values
   //xat and yat, return zat
   double zat;
   //set xdata and ydata from input data points
   
   ofstream cfout;
   cfout.open ("coeffs.txt" );
   double tmp;
   vector<double> xdata, ydata;
   vector<double> mu;   //this is the value of the splines evaluated at xat
   //NOTE: xdata and ydata are generic representations for fitting the splines
   //they do not have anything to do with the coordinate system
   //create a series of 1D splines to pass through each y_j
   for ( int j=0;j<y.size();j++ )
   {
      if ( DBG_BICUBIC > 1 )
         cout<<"Evaluating spline at j:"<<j<<endl;
      //now set xdata and ydata
      for ( int i=0;i<x.size();i++ )
      {
         xdata.push_back( x.at(i) );
         ydata.push_back( gsl_matrix_get(z, i, j ) );
      }
      spline1D = new Interpolation( );
      spline1D->SetInputPoints1D( xdata, ydata );
      //get coefficients and write them out
      vector < gsl_vector* > coeffs;
      coeffs = spline1D->GetCubicSplineCoefficientVectors( );
      tmp = spline1D->EvaluateCubicSplineAt( xat );
      mu.push_back( tmp );
      
      if ( DBG_BICUBIC > 1 )
         cout<<"C-Spline interpolation value is :"<<tmp<<endl;
            
      gsl_vector* c3 = coeffs.at( 0 );
      gsl_vector* c2 = coeffs.at( 1 );
      gsl_vector* c1 = coeffs.at( 2 );
      gsl_vector* c0 = coeffs.at( 3 );
      
      for ( int count=0;count<c3->size;count++ )
         cfout<<count<<"\t"<<gsl_vector_get(c3,count)<<"\t"<<
               gsl_vector_get(c2,count)<<"\t"<<gsl_vector_get(c1,count)<<"\t"
               <<gsl_vector_get(c0,count)<<endl;

      //clean up for next set of points
      delete spline1D;
      spline1D = NULL;
      
      xdata.clear();
      ydata.clear();
   }
   //now fit splines through the mu points in the y direction
   //std::assert( (mu.size() == y.size()) );
   if( mu.size() == y.size() )
   {
       std::cout << "assert failure" << std::endl;
       exit( 0 );
   }
   ydata.resize( mu.size() );
   xdata.resize( mu.size() );
   if ( DBG_BICUBIC > 1 )
      cout<<"  Size of MU :"<<mu.size()<<endl;
   for ( int j=0;j<mu.size();j++ )
   {
      xdata.at(j) = y.at(j);
      ydata.at(j) = mu.at(j);
   }
   if ( DBG_BICUBIC > 1 )
   {
      cout<<"BICUBIC_INTERPOLATION :"<<endl<<"xdata"<<"\t"<<"ydata"<<endl;
      for ( int j=0;j<mu.size();j++ )
      {
         cout<<xdata.at(j)<<"\t"<<ydata.at(j)<<endl;
      }
   }
   //fit splines
   spline1D = new Interpolation( );
   spline1D->SetInputPoints1D( xdata, ydata );

   vector < gsl_vector* > coeffs;
   coeffs = spline1D->GetCubicSplineCoefficientVectors( );
   gsl_vector* c3 = coeffs.at( 0 );
   gsl_vector* c2 = coeffs.at( 1 );
   gsl_vector* c1 = coeffs.at( 2 );
   gsl_vector* c0 = coeffs.at( 3 );
   if ( DBG_BICUBIC > 1 )
   {
      for ( int count=0;count<c3->size;count++ )
         cout<<count<<"\t"<<gsl_vector_get(c3,count)<<"\t"<<
               gsl_vector_get(c2,count)<<"\t"<<gsl_vector_get(c1,count)<<"\t"
               <<gsl_vector_get(c0,count)<<endl;
   }
   
   int evalAt = this->GetPosition( yat, xdata );
   zat = gsl_vector_get(c3, evalAt)*pow((yat-xdata.at(evalAt)),3.0) + 
            gsl_vector_get(c2, evalAt)*pow((yat-xdata.at(evalAt)),2.0) + 
            gsl_vector_get(c1, evalAt)*(yat-xdata.at(evalAt)) + 
         
            gsl_vector_get(c0, evalAt);
         
   if ( DBG_BICUBIC > 1 )
      
   {
      cout<<"(X,Y,Z) : ("<<xat<<","<<yat<<","<<zat<<")"<<endl;
      cout<<"(X,Y,Z) : ("<<xat<<","<<yat<<","<<
            sin(xat*xat) - cos(yat*yat)<<")"<<endl;
   }
   cfout.close( );
   return zat;
}
////////////////////////////////////////////////////////////////////////////////

